// ------------------------------------------------------
//  Refer to the description in the wiki:
//  http://babel.isa.uma.es/mrpt/Kalman_Filters
// ------------------------------------------------------

#include <mrpt/core.h>

#include "SimulationManager.h"

using namespace mrpt;
using namespace mrpt::bayes;
using namespace mrpt::gui;
using namespace mrpt::math;
using namespace mrpt::slam;
using namespace mrpt::utils;
using namespace std;

#define BEARING_SENSOR_NOISE_STD 	DEG2RAD(15.0f)
#define RANGE_SENSOR_NOISE_STD 		0.3f

#define DELTA_TIME 0.1f

#define NUM_PARTICLES 2000


void TestBayesianTracking() {

	mrpt::random::Randomize();

	SimulationManager simulationManager;

	simulationManager.setNumParticles(NUM_PARTICLES);

	simulationManager.setDeltaTime(DELTA_TIME);

	CParticleVehicleData initialVehicleData;
	initialVehicleData.x  = 4.0f;
	initialVehicleData.y  = 4.0f;
	initialVehicleData.w  = DEG2RAD(20.0f);
	initialVehicleData.vx = 1.0f;
	initialVehicleData.vy = 1.0f;
	initialVehicleData.vw = DEG2RAD(2.0f);

	simulationManager.setInitialVehicleData(initialVehicleData);

	CParticleVehicleData stdDev;
	initialVehicleData.x  = 1.0f;
	initialVehicleData.y  = 1.0f;
	initialVehicleData.w  = DEG2RAD(2.0f);
	initialVehicleData.vx = 1.0f;
	initialVehicleData.vy = 1.0f;
	initialVehicleData.vw = DEG2RAD(2.0f);

	simulationManager.setVehicleDataStdDev(stdDev);

	LandmarkMap landmarkMap;
	int id = 0;
	for (int i = 0; i < 10; i++) {
		for (int j = 0; j < 10; j++) {
			landmarkMap.addLandmark(id++, cvPoint2D32f(i, j - 5.0f));
		}
	}

	simulationManager.setLandmarkMap(landmarkMap);

	Camera1DInternal camInternal(100, 100);

	simulationManager.setCamera1DInternal(camInternal);

	simulationManager.run();
}

int main()
{
	try
	{
		TestBayesianTracking();
		return 0;
	} catch (std::exception &e)
	{
		std::cout << "MRPT exception caught: " << e.what() << std::endl;
		return -1;
	}
	catch (...)
	{
		printf("Untyped exception!!");
		return -1;
	}
}





